/*
    James McQueen, Tyson Nottingham, Michael Pogson
    TCSS 372 Winter 2012
    SC2 Simulator
    
    cpu.c
*/

#include "cpu.h"

/* Tests each of the instructions.
int main() {

	CPUPtr cpu = cpu_initialize();
	printBanner("LOAD TESTS");
	testLD(cpu);
	printBanner("STORE TESTS");
	testST(cpu);
	printBanner("PUSH POP TESTS");
	testPushPop(cpu);
	printBanner("BRANCH TESTS");
	testBranch(cpu);
	printBanner("JUMP TESTS");
	testJumps(cpu);
    
	return 0;
}*/

/* CPU constructor.*/
CPUPtr cpu_initialize() {
    CPUPtr cpu = malloc(sizeof(*cpu));
    cpu->rf = registerFile_initialize();
    cpu->alu = alu_initialize(&cpu->SW, cpu->rf);
    cpu->mm = memory_initialize();
    cpu->macroState = STOP;
    cpu->ioport[KBD_STATUS] = 0;
    cpu->ioport[VID_STATUS] = 0;
    register_putValue(&cpu->PC, MEMORY_START_ADDRESS);
    return cpu;
}

/* Executes one instruction cycle. Returns an error code.*/
int cpu_instructionCycle(CPUPtr cpu) {
    int done = FALSE;
    while (!done) {
        int err = NO_ERROR;
        switch (cpu->macroState) {
            case FETCH:
                err = cpu_fetch(cpu);
                if (err != NO_ERROR) {
                    return err;
                }
                break;
            case DECODE:
                err = cpu_decode(cpu);
                if (err != NO_ERROR) {
                    return err;
                }
                break;
            case EXECUTE:
                err = cpu_execute(cpu);
                if (err != NO_ERROR) {
                    return err;
                }
                done = TRUE;
                break;
            default:    // Bug in CPU ADT if this code is executed.
                printf("Invalid macroState in cpu_instructionCycle.");
                getchar();
                exit(1);
                break;
        }
    }

    return NO_ERROR;
}

/* Fetches the instruction pointed to by the PC, places it into the IR, and
   increments the PC by one word.*/
int cpu_fetch(CPUPtr cpu) {
    if (cpu == NULL) {
        return NULL_POINTER_ERROR;
    }
    
    /*  PC -> MAR;
        mem[MAR] -> MDR;
        MDR -> IR; */
    register_putValue(&cpu->MAR, cpu_getPC(cpu));
    ushort instruction = memory_getWord(cpu->mm, register_getValue(&cpu->MAR, &error), &error);
    int err = error;
    if (err == NO_ERROR) {
        register_putValue(&cpu->MDR, instruction);
        register_putValue(&cpu->IR, register_getValue(&cpu->MDR, &error));
        cpu_setPC(cpu, cpu_getPC(cpu) + 2);
        cpu->macroState = DECODE;
    }
    
    return err;
}

/* Decodes the instruction in the IR and sets the the operation to be
   executed in the EXECUTE macrostate.*/
int cpu_decode(CPUPtr cpu) {
    if (cpu == NULL) {
        return NULL_POINTER_ERROR;
    }

    cpu_setOperands(cpu);

    switch (cpu->operand[OPCODE]) {
        case LDI: cpu->operation = cpu_ldi; break;
        case LDA: cpu->operation = cpu_lda; break;
        case LDB: cpu_decodeLDB(cpu); break;
        case LDW: cpu_decodeLDW(cpu); break;
        case STB: cpu_decodeSTB(cpu); break;
        case STW: cpu_decodeSTW(cpu); break;
        case PUSH_OR_POP: cpu_decodePUSHorPOP(cpu); break;
        case ALU_OPCODE: case SHL_OPCODE: case SHR_OPCODE: cpu->operation = cpu_aluOperation; break;
        case BRI: cpu->operation = cpu_bri; break;
        case BR: cpu->operation = cpu_br; break;
        case BRIX: cpu->operation = cpu_brix; break;
        case BRX: cpu->operation = cpu_brx; break;
        case JSR: cpu->operation = cpu_jsr; break;
        case JSRX: cpu->operation = cpu_jsrx; break;
        case TRAP: cpu->operation = cpu_trap; break;
        case RET_OR_RETI: cpu_decodeRETorRETI(cpu); break;
        case IN_OR_OUT: cpu_decodeINorOUT(cpu); break;
        case EI_OR_DI: cpu_decodeEIorDI(cpu); break;
        case HALT: cpu->operation = cpu_halt; break;
        case LDOS: cpu->operation = cpu_ldos; break;
        case LDRO: cpu->operation = cpu_ldro; break;
        case NOP: break;
        default: return INVALID_INSTRUCTION_ERROR; break;
    }
    
    cpu->macroState = EXECUTE;
    
    return NO_ERROR;
}

/* Retreives the operands from the instruction in the IR.*/
int cpu_setOperands(CPUPtr cpu) {
    /* Decode every field in the instruction. Some fields are
       transformed into more usable data. */
    ushort instruction = register_getValue(&cpu->IR, &error);
    cpu->operand[OPCODE] = (instruction & OPCODE_MASK) >> OPCODE_SHIFT;
    cpu->operand[REG_OP1] = (instruction & REG_OP1_MASK) >> REG_OP1_SHIFT;
    cpu->operand[REG_OP2] = (instruction & REG_OP2_MASK) >> REG_OP2_SHIFT;
    cpu->operand[TRAPV] = (instruction & TRAPV_MASK) >> TRAPV_SHIFT;
    cpu->operand[IO_PORT] = (instruction & IO_PORT_MASK) >> IO_PORT_SHIFT;
    cpu->operand[IRQ] = (instruction & IRQ_MASK) >> IRQ_SHIFT;
    cpu->operand[IMMED9] = (instruction & IMMED9_MASK) >> IMMED9_SHIFT;
    cpu->operand[IMMED7] = (instruction & IMMED7_MASK);
    cpu->operand[IMMED11] = (instruction & IMMED11_MASK);
    cpu->operand[ADDR_MODE] = (instruction & ADDR_MODE_MASK);
    cpu->operand[STACK_MODE] = (instruction & STACK_MODE_MASK);
    cpu->operand[BR_MODE] = 1 << (15 - (instruction & BR_MODE_MASK));
    cpu->operand[RET_MODE] = (instruction & RET_MODE_MASK);
    cpu->operand[IO_MODE] = (instruction & IO_MODE_MASK);
    cpu->operand[IRQ_MODE] = (instruction & IRQ_MASK);
    
    if (cpu->operand[OPCODE] == SHL_OPCODE) {
        cpu->operand[ALU_OPERATION] = SHL;
    } else if (cpu->operand[OPCODE] == SHL_OPCODE) {
        cpu->operand[ALU_OPERATION] = SHR;
    } else {
        cpu->operand[ALU_OPERATION] = (instruction & ALU_OPERATION_MASK);
    }
    
    return NO_ERROR;
}

/* Sets the type of load-byte instruction to be executed according to the
   addressing mode.*/
int cpu_decodeLDB(CPUPtr cpu) {
    switch (cpu->operand[ADDR_MODE]) {
        case REGISTER: cpu->operation = cpu_ldbRegister; break;
        case BASE_RELATIVE: cpu->operation = cpu_ldbBaseRelative; break;
        case INDEX: cpu->operation = cpu_ldbIndex; break;
        case INDIRECT: cpu->operation = cpu_ldbIndirect; break;
        default: return INVALID_INSTRUCTION_ERROR;
    }
    
    return NO_ERROR;
}

/* Sets the type of load-word instruction to be executed according to the
   addressing mode.*/
int cpu_decodeLDW(CPUPtr cpu) {
    switch (cpu->operand[ADDR_MODE]) {
        case REGISTER: cpu->operation = cpu_ldwRegister; break;
        case BASE_RELATIVE: cpu->operation = cpu_ldwBaseRelative; break;
        case INDEX: cpu->operation = cpu_ldwIndex; break;
        case INDIRECT: cpu->operation = cpu_ldwIndirect; break;
        case REGISTER_MOVE: cpu->operation = cpu_mov; break;
        default: return INVALID_INSTRUCTION_ERROR;
    }
    
    return NO_ERROR;
}

/* Sets the type of store-byte instruction to be executed according to the
   addressing mode.*/
int cpu_decodeSTB(CPUPtr cpu) {
    switch (cpu->operand[ADDR_MODE]) {
        case REGISTER: cpu->operation = cpu_stbRegister; break;
        case BASE_RELATIVE: cpu->operation = cpu_stbBaseRelative; break;
        case INDEX: cpu->operation = cpu_stbIndex; break;
        case INDIRECT: cpu->operation = cpu_stbIndirect; break;
        default: return INVALID_INSTRUCTION_ERROR;
    }
    
    return NO_ERROR;
}

/* Sets the type of store-word instruction to be executed according to the
   addressing mode.*/
int cpu_decodeSTW(CPUPtr cpu) {
    switch (cpu->operand[ADDR_MODE]) {
        case REGISTER: cpu->operation = cpu_stwRegister; break;
        case BASE_RELATIVE: cpu->operation = cpu_stwBaseRelative; break;
        case INDEX: cpu->operation = cpu_stwIndex; break;
        case INDIRECT: cpu->operation = cpu_stwIndirect; break;
        default: return INVALID_INSTRUCTION_ERROR;
    }
    
    return NO_ERROR;
}

/* Sets the type of stack operation to be executed.*/
int cpu_decodePUSHorPOP(CPUPtr cpu) {
    switch (cpu->operand[STACK_MODE]) {
        case PUSHB: cpu->operation = cpu_pushb; break;
        case POPB: cpu->operation = cpu_popb; break;
        case PUSHW: cpu->operation = cpu_pushw; break;
        case POPW: cpu->operation = cpu_popw; break;
        default: return INVALID_INSTRUCTION_ERROR;
    }
    
    return NO_ERROR;
}

/* Sets the type of return operation to be executed.*/
int cpu_decodeRETorRETI(CPUPtr cpu) {
    switch (cpu->operand[RET_MODE]) {
        case RET: cpu->operation = cpu_ret; break;
        case RETI: cpu->operation = cpu_reti; break;
        default: return INVALID_INSTRUCTION_ERROR;
    }
    
    return NO_ERROR;
}

/* Sets the type of I/O operation to be executed.*/
int cpu_decodeINorOUT(CPUPtr cpu) {
    switch (cpu->operand[IO_MODE]) {
        case IN_MODE: cpu->operation = cpu_in; break;
        case OUT_MODE: cpu->operation = cpu_out; break;
    }
    
    return NO_ERROR;
}

/* Sets the operation to enable interrupt or disable interrupt.*/
int cpu_decodeEIorDI(CPUPtr cpu) {
    switch (cpu->operand[IRQ_MODE]) {
        case ENABLE_MODE: cpu->operation = cpu_ei; break;
        case DISABLE_MODE: cpu->operation = cpu_di; break;
    }
    
    return NO_ERROR;
}

/* Executes the operation determined by the DECODE macrostate.*/
int cpu_execute(CPUPtr cpu) {
    int err = cpu->operation(cpu);
    
    if (cpu->macroState != STOP) {
        cpu->macroState =FETCH;
    }
    
    return err;
}

/* Loads a 7-bit immediate operand into a register.*/
int cpu_ldi(CPUPtr cpu) {
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], sext(cpu->operand[IMMED7], 7));
    return NO_ERROR;
}

/* Loads a 16-bit immediate operand into a register.*/
int cpu_lda(CPUPtr cpu) {
	ushort pc = register_getValue(&cpu->PC, &error);
    
    registerFile_putHighByte(cpu->rf, cpu->operand[REG_OP1], memory_getByte(cpu->mm, pc, &error));
    
    if (error != NO_ERROR) {
        return error;
    }
    
	registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], memory_getByte(cpu->mm, ++pc, &error));
    
    if (error != NO_ERROR) {
        return error;
    }
    
	register_putValue(&cpu->PC, ++pc);
    return NO_ERROR;
}

/* Loads a byte from the memory location specified by a register into the
   low order byte of another register.*/
int cpu_ldbRegister(CPUPtr cpu) {
    ushort addr = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error);
    uchar data = memory_getByte(cpu->mm, addr, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], data);
    return NO_ERROR;
}

/* Loads a byte from the memory location specified by a register plus the base
   pointer register into the low order byte of another register.*/
int cpu_ldbBaseRelative(CPUPtr cpu) {
    ushort addr = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error) + registerFile_getWord(cpu->rf, BP, &error);
    uchar data = memory_getByte(cpu->mm, addr, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], data);
    
    return NO_ERROR;
}

/* Loads a byte from the memory location specified by the IP0 register into the
   low order byte of another register, then increments the IP0 register by one.*/
int cpu_ldbIndex(CPUPtr cpu) {
	ushort ip = registerFile_getWord(cpu->rf, IP0, &error);
    ushort data = memory_getByte(cpu->mm, ip, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], data);
	registerFile_putWord(cpu->rf, IP0, ++ip);
    return NO_ERROR;
}

/* Loads a byte from the memory location specified by the contents of the
   memory location specified by a register into the low order byte of another
   register.*/
int cpu_ldbIndirect(CPUPtr cpu) {
    ushort addr1 = registerFile_getWord(cpu->rf , cpu->operand[REG_OP2], &error);
    ushort addr2 = memory_getWord(cpu->mm, addr1, &error);
    uchar data = memory_getByte(cpu->mm, addr2, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], data);
    return NO_ERROR;
}

/* Loads a word from the memory location specified by a register into another
   register.*/
int cpu_ldwRegister(CPUPtr cpu) {
    ushort addr = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error);
    uchar hob = memory_getByte(cpu->mm, addr, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    uchar lob = memory_getByte(cpu->mm, addr + 1, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putHighByte(cpu->rf, cpu->operand[REG_OP1], hob);
	registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], lob);
    return NO_ERROR;
}

/* Loads a word from the memory location specified by a register plus the base
   pointer register into another register.*/
int cpu_ldwBaseRelative(CPUPtr cpu) {
	ushort addr = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error) + registerFile_getWord(cpu->rf, BP, &error);
    uchar hob = memory_getByte(cpu->mm, addr, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    uchar lob = memory_getByte(cpu->mm, addr + 1, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putHighByte(cpu->rf, cpu->operand[REG_OP1], hob);
	registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], lob);
    return NO_ERROR;
}

/* Loads a word from the memory location specified by the IP0 register into
   another register, then increments the IP0 register by two.*/
int cpu_ldwIndex(CPUPtr cpu) {
	ushort ip = registerFile_getWord(cpu->rf, IP0, &error);
	uchar hob = memory_getByte(cpu->mm, ip++, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    uchar lob = memory_getByte(cpu->mm, ip++, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putHighByte(cpu->rf, cpu->operand[REG_OP1], hob);
	registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], lob);
	registerFile_putWord(cpu->rf, IP0, ip);
    return NO_ERROR;
}

/* Loads a word from the memory location specified by the contents of the
   memory location specified by a register into another register.*/
int cpu_ldwIndirect(CPUPtr cpu) {
    ushort addr1 = registerFile_getWord(cpu->rf , cpu->operand[REG_OP2], &error);
    ushort addr2 = memory_getWord(cpu->mm, addr1, &error);
    uchar hob = memory_getByte(cpu->mm, addr2, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    uchar lob = memory_getByte(cpu->mm, addr2 + 1, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putHighByte(cpu->rf, cpu->operand[REG_OP1], hob);
	registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], lob);
    return NO_ERROR;
}

/* Loads a register with the contents of another register.*/
int cpu_mov(CPUPtr cpu) {
	ushort value = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error);
    registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], value);
    return NO_ERROR;
}

/* Stores the low order byte of a register to the memory location specified by
   another register.*/
int cpu_stbRegister(CPUPtr cpu) {
    ushort addr = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error);
    uchar data = registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error);

    return memory_putByte(cpu->mm, addr, data);
}

/* Stores the low order byte of a register to the memory location specified by
   another register plus the base pointer register.*/
int cpu_stbBaseRelative(CPUPtr cpu) {
    ushort addr = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error) +
                  registerFile_getWord(cpu->rf, BP, &error);
    uchar data = registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error);

    return memory_putByte(cpu->mm, addr, data);
}

/* Stores the low order byte of a register to the memory location specified by
   the IP1 register, then increments the IP1 register.*/
int cpu_stbIndex(CPUPtr cpu) {
    int err = cpu_stbRegister(cpu);
    registerFile_putWord(cpu->rf, IP1, registerFile_getWord(cpu->rf, IP1, &error) + 1);
    return err;
}

/* Stores the low order byte of a register to the memory location specified by
   the contents of the memory location specified by another register.*/
int cpu_stbIndirect(CPUPtr cpu) {
    ushort addr1 = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error);
    ushort addr2 = memory_getWord(cpu->mm, addr1, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    uchar data = registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error);

    return memory_putByte(cpu->mm, addr2, data);
}

/* Stores the contents of a register to the memory location specified by
   another register.*/
int cpu_stwRegister(CPUPtr cpu) {
    ushort addr = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error);
    ushort data = registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error);

    return memory_putWord(cpu->mm, addr, data);
}

/* Stores the contents of a register to the memory location specified by
   another register plus the base pointer register.*/
int cpu_stwBaseRelative(CPUPtr cpu) {
    ushort addr = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error) +
                  registerFile_getWord(cpu->rf, BP, &error);
    ushort data = registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error);

    return memory_putWord(cpu->mm, addr, data);
}

/* Stores the contents of a register to the memory location specified by
   the IP1 register, then increments the IP1 register.*/
int cpu_stwIndex(CPUPtr cpu) {
    int err = cpu_stwRegister(cpu);
    registerFile_putWord(cpu->rf, IP1, registerFile_getWord(cpu->rf, IP1, &error) + 2);
    return err;
}

/* Stores the contents of a register to the memory location specified by
   the contents of the memory location specified by another register.*/
int cpu_stwIndirect(CPUPtr cpu) {
    ushort addr1 = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error);
    ushort addr2 = memory_getWord(cpu->mm, addr1, &error);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    ushort data = registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error);

    return memory_putWord(cpu->mm, addr2, data);
}

/* Stores the low order byte of a register to the memory location specified by
   the pre-decremented stack pointer register. The stack pointer register is
   updated with the decremented value.*/
int cpu_pushb(CPUPtr cpu) {
    ushort sp = registerFile_getWord(cpu->rf, SP, &error);
    uchar data = registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error);
    error = memory_putByte(cpu->mm, --sp, data); /* Pre-dec push. */
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putWord(cpu->rf, SP, sp); /* Update stack pointer. */
    
    return NO_ERROR;
}

/* Loads a byte from the memory location specified by the stack pointer
   register into the low order byte of another register, then increments the
   stack pointer register by one.*/
int cpu_popb(CPUPtr cpu) {
    ushort sp = registerFile_getWord(cpu->rf, SP, &error);
    uchar data = memory_getByte(cpu->mm, sp++, &error); /* Post-inc pop. */
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], data);
    registerFile_putWord(cpu->rf, SP, sp); /* Update stack pointer. */
    
    return NO_ERROR;
}

/* Stores the contents of a register to the memory location specified by
   the pre-decremented stack pointer register. The stack pointer register is
   updated with the decremented value.*/
int cpu_pushw(CPUPtr cpu) {
    ushort sp = registerFile_getWord(cpu->rf, SP, &error);
    ushort data = registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error);
    sp -= 2;
    memory_putWord(cpu->mm, sp, data); /* Actually two putBytes. */
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putWord(cpu->rf, SP, sp); /* Update stack pointer. */
    
    return NO_ERROR;
}

/* Loads a word from the memory location specified by the stack pointer
   register into the low order byte of another register, then increments the
   stack pointer register by one.*/
int cpu_popw(CPUPtr cpu) {
    ushort sp = registerFile_getWord(cpu->rf, SP, &error);
    ushort data = memory_getWord(cpu->mm, sp, &error); /* Actually two getBytes. */
    
    if (error != NO_ERROR) {
        return error;
    }
    
    sp += 2;
    registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], data);
    registerFile_putWord(cpu->rf, SP, sp); /* Update stack pointer. */
    
    return NO_ERROR;
}

/* Executes an ALU operation and places the result in the register file.*/
int cpu_aluOperation(CPUPtr cpu) {
    ushort op1 = registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error);
    ushort op2 = registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error);
    
    alu_execute(cpu->alu, cpu->operand[ALU_OPERATION], op1, op2);
    
    if (cpu->operand[ALU_OPERATION] != MUL && cpu->operand[ALU_OPERATION] != DIV) {
        registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], alu_getResult(cpu->alu, &error));
    }
    
    return NO_ERROR;
}

/* Unconditionally sets the PC to the sum of the (already incremented) PC and
   the two's complement 11-bit offset specified in the instruction.*/
int cpu_bri(CPUPtr cpu) {
    ushort addr = register_getValue(&cpu->PC, &error) + sext(cpu->operand[IMMED11], 11);
    register_putValue(&cpu->PC, addr);
    
    return NO_ERROR;
}

/* Unconditionally sets the PC to the value obtained from a register.*/
int cpu_br(CPUPtr cpu) {
    ushort addr = registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error);
    register_putValue(&cpu->PC, addr);
    
    return NO_ERROR;
}

/* Conditionally sets the PC to the sum of the (already incremented) PC and
   the two's complement 11-bit offset specified in the instruction.*/
int cpu_brix(CPUPtr cpu) {
    if (register_getValue(&cpu->SW, &error) & cpu->operand[BR_MODE]) {
        ushort addr = register_getValue(&cpu->PC, &error) + sext(cpu->operand[IMMED9], 9);
        register_putValue(&cpu->PC, addr);
    }
	
    return NO_ERROR;
}

/* Conditionally sets the PC to the value from a register.*/
int cpu_brx(CPUPtr cpu) {
    if (register_getValue(&cpu->SW, &error) & cpu->operand[BR_MODE]) {
        cpu_br(cpu);
    }
    
    return NO_ERROR;
}

/* Unconditionally pushes the current PC onto the stack and sets the PC to
   a value obtained from a register.*/
int cpu_jsr(CPUPtr cpu) {
    ushort sp = registerFile_getWord(cpu->rf, SP, &error) - 2;
    ushort pc = register_getValue(&cpu->PC, &error); 
    memory_putWord(cpu->mm, sp, pc); /* Push the PC. */
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putWord(cpu->rf, SP, sp); /* Update stack pointer. */
    ushort addr = registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error);
    register_putValue(&cpu->PC, addr); /* Update the PC. */
    
    return NO_ERROR;
}

/* Conditionally pushes the current PC onto the stack and sets the PC to
   a value obtained from a register.*/
int cpu_jsrx(CPUPtr cpu) {
    if (register_getValue(&cpu->SW, &error) & cpu->operand[BR_MODE]) {
        cpu_jsr(cpu);
    }
    
    return NO_ERROR;
}

int cpu_trap(CPUPtr cpu) {
    ushort sp = registerFile_getWord(cpu->rf, SP, &error);
    ushort data = register_getValue(&cpu->PC, &error);
    sp -= 2;
    memory_putWord(cpu->mm, sp, data); /* Actually two putBytes. */
    
    if (error != NO_ERROR) {
        return error;
    }
    
    data = register_getValue(&cpu->SW, &error);
    sp -= 2;
    memory_putWord(cpu->mm, sp, data);
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putWord(cpu->rf, SP, sp); /* Update stack pointer. */
    cpu->PC = IVT + sext(cpu->operand[TRAPV], 6);
    
    return NO_ERROR;
}

/* Pops a word from the stack and places it in the PC.*/
int cpu_ret(CPUPtr cpu) {
    ushort sp = registerFile_getWord(cpu->rf, SP, &error);
    ushort pc = memory_getWord(cpu->mm, sp, &error); /* Pop PC. */

    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putWord(cpu->rf, SP, sp + 2); /* Update stack pointer. */
    register_putValue(&cpu->PC, pc); /* Update PC. */
    
    return NO_ERROR;
}

/* Pops a word from the stack and places it in the SW. Then pops another word
   from the stack and places it in the PC.*/
int cpu_reti(CPUPtr cpu) {
    ushort sp = registerFile_getWord(cpu->rf, SP, &error);
    ushort sw = memory_getWord(cpu->mm, sp, &error); /* Pop SW. */
    
    if (error != NO_ERROR) {
        return error;
    }
    
    registerFile_putWord(cpu->rf, SP, sp + 2); /* Update stack pointer. */
    register_putValue(&cpu->SW, sw); /* Update SW. */
    cpu_ret(cpu);
    
    return NO_ERROR;
}

/* Gets data from an I/O port and places it in the register file.*/
int cpu_in(CPUPtr cpu) {
    if (cpu->operand[IO_PORT] == KBD_STATUS) {
        /* If IN called on KBD status register, assume that the program is waiting
           for user input. Get character input and put into KBD data register.*/
        cpu->ioport[KBD_DATA0] = getchar();
        
        /* Clear out all but first character entered by user.*/
        if (cpu->ioport[KBD_DATA0] != '\n') {
            while (getchar() != '\n')
                ;
        }
        
        /* Set the register to 1 to indicate a character is ready to be read.*/
        registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], 1);
    } else {
        /* Else just put the data from the port into the register file.*/
        registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], cpu->ioport[cpu->operand[IO_PORT]]);
    }
    
    return NO_ERROR;
}

/* Puts data from a register into an I/O port.*/
int cpu_out(CPUPtr cpu) {
    cpu->ioport[cpu->operand[IO_PORT]] = registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error);
    
    if (cpu->operand[IO_PORT] == VID_DATA) {
        /* Set VID status register to 1 to indicate that the character has not
           yet been written to the display. cpu_getVideo() will clear this.*/
        cpu->ioport[VID_STATUS] = 1; 
    }
    
    return NO_ERROR;
}

int cpu_ei(CPUPtr cpu) {
    ushort enabled = cpu->operand[IRQ];
    cpu->SW = enabled | cpu->SW;
    
    return NO_ERROR;
}

int cpu_di(CPUPtr cpu) {
    ushort enabled = cpu->operand[IRQ];
    register_putLowByte(&cpu->SW, enabled & cpu->SW);
    
    return NO_ERROR;
}

/* Halts the CPU.*/
int cpu_halt(CPUPtr cpu) {
    cpu->macroState = STOP;
    
    return NO_ERROR;
}


int cpu_ldos(CPUPtr cpu) {
    cpu->OSB = (sext(cpu->operand[IMMED11], 11) + cpu->PC);
    
    return NO_ERROR;
}

int cpu_ldro(CPUPtr cpu) {
    cpu->TXB = (sext(cpu->operand[IMMED11], 11) + cpu->PC);
    
    return NO_ERROR;
}

/* Returns a RegisterFilePtr for this CPU.*/
RegisterFilePtr cpu_getRegisterFile(CPUPtr cpu) {
    return cpu->rf;
}

/* Returns a MemoryModulePtr for this CPU.*/
MemoryModulePtr cpu_getMemoryModule(CPUPtr cpu) {
    return cpu->mm;
}

/* Returns whether or not there was a character written to video since the last
   call to getVideo().*/
int cpu_isVideoWaiting(CPUPtr cpu) {
    return cpu->ioport[VID_STATUS];
}

/* Gets the last character written to the video output device.*/
uchar cpu_getVideo(CPUPtr cpu) {
    cpu->ioport[VID_STATUS] = 0;
    return cpu->ioport[VID_DATA];
}

/* Returns the value of the SW.*/
ushort cpu_getSW(CPUPtr cpu) {
    return cpu->SW;
}

/* Returns the value of the N condition code.*/
ushort cpu_getN(CPUPtr cpu) {
    return (cpu->SW & N_MASK) != 0;
}

/* Returns the value of the Z condition code.*/
ushort cpu_getZ(CPUPtr cpu) {
    return (cpu->SW & Z_MASK) != 0;
}

/* Returns the value of the C condition code.*/
ushort cpu_getC(CPUPtr cpu) {
    return (cpu->SW & C_MASK) != 0;
}

/* Returns the value of the O condition code.*/
ushort cpu_getO(CPUPtr cpu) {
    return (cpu->SW & O_MASK) != 0;
}

/* Returns the value of the PC.*/
ushort cpu_getPC(CPUPtr cpu) {
    return cpu->PC;
}

/* Returns the value of the IR.*/
ushort cpu_getIR(CPUPtr cpu) {
    return cpu->IR;
}

/* Returns the value of the MDR.*/
ushort cpu_getMDR(CPUPtr cpu) {
    return cpu->MDR;
}

/* Returns the value of the MAR.*/
ushort cpu_getMAR(CPUPtr cpu) {
    return cpu->MAR;
}

/* Sets the value of the SW.*/
void cpu_setSW(CPUPtr cpu, ushort sw) {
    register_putValue(&cpu->SW, sw);
}

/* Sets the value of the PC.*/
void cpu_setPC(CPUPtr cpu, ushort pc) {
    register_putValue(&cpu->PC, pc);
}

/* Sets the value of the IR.*/
/* This is useless.*/
void cpu_setIR(CPUPtr cpu, ushort ir) {
    register_putValue(&cpu->IR, ir);
}

/* Returns the macrostate of the CPU.*/
int cpu_getState(CPUPtr cpu) {
    return cpu->macroState;
}

/* Resets the state of the CPU.*/
void cpu_reset(CPUPtr cpu) {
    cpu->macroState = FETCH;
}

/* Sign extension helper function.
   data: the data to sign extend.
   num_bits: the width of the data before sign extension.
*/
ushort sext(ushort data, uchar num_bits) {
    ushort sign_mask = 1 << (num_bits - 1);
    return (data ^ sign_mask) - sign_mask;
    /* Algorithm:
       1. Compute the sign mask.
       2. Toggle the sign bit of the data.
       3. Negate the sign mask. Because the sign mask is a single bit,
          the negation will be all ones from the sign bit through the high
          order bits, and all zeros below the sign bit.
       4. Add the negated sign mask to the data with the toggled sign bit.
          If the original sign bit was one, then it's zero in the toggled data.
          Adding the negated sign mask fills the vacant bits from the sign bit up.
          If the original sign bit was zero, then it's one in the toggled data.
          Adding the negated sign mask produces zeroes because the sign bit in
          the toggled data carries through the addition until the end of the bit
          pattern.
        
        Example:
        0000 0101 0101 0101  Negative 11 bit 2's complement number
        0000 0100 0000 0000  Computed sign mask
        0000 0001 0101 0101  data ^ sign_mask (toggle the sign bit)
        1111 1100 0000 0000  -m
        1111 1101 0101 0101  data ^ sign_mask + (-m) (sign extended)
    */
}

/* Tests the load instructions.*/
void testLD(CPUPtr cpu) {
	
	uchar regnum  = RA;
	uchar regnum2 = RB;
    ushort address = 0x8100;
	ushort immed7 = 0x68;
	
	cpu->operand[REG_OP1] = RA;
	cpu->operand[IMMED7] = immed7;
	printf("\nTesting the LDI execution\n");
	printf("Using register %u for the end result. putting %x into that register", regnum, sext(immed7,7));
	printf("\nbefore the load this is what was in the register: %x ",registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error)); 
	
	cpu_ldi(cpu);
	
	printf("\nAfter the load this is what was in the register: %x \n",registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error)); 
	
	
	printf("\n\nTesting the LDA execution\n");
	cpu->PC = address;
	uchar low = 0x42;
    uchar high = 0xf0;
	memory_putByte(cpu->mm, cpu->PC, high);
	memory_putByte(cpu->mm, cpu->PC+1, low);
	printf("Using register %u for the end result. putting %x into high order byte, and %x into low order byte", regnum, memory_getByte(cpu->mm, cpu->PC , &error),memory_getByte(cpu->mm, cpu->PC+1 , &error));
	cpu_lda(cpu);
	printf("\nThis is what was in register %u: %x\n",regnum, registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	
	printf("\n\nTesting the LDB_register execution\n");
	
	regnum = RB;
	regnum2 = RC;
	address = 0x8000;
	low = 0xab;
	memory_putByte(cpu->mm, address, low);
	cpu->operand[REG_OP2] = regnum2;
	registerFile_putWord(cpu->rf,cpu->operand[REG_OP2], address);
	cpu->operand[REG_OP1] = regnum;
	registerFile_putWord(cpu->rf,cpu->operand[REG_OP1], 0xffff);
	
	printf("Register %u has %x in the low byte before the load.\n", regnum, registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("The register should contain %x in the low byte after the load!\n", memory_getByte(cpu->mm, registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error), &error));
	cpu_ldbRegister(cpu);
	printf("Register %u has %x in it after the load.\n", regnum, registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	
	printf("\n\nTesting the LDB_BASE_RELATIVE execution\n");
	
	regnum = R1;
	regnum2 = R2;
	address = 0x8300;
	low = 0xab;
	ushort bp = registerFile_getWord(cpu->rf, BP, &error);
	memory_putByte(cpu->mm, address + bp, low);
	cpu->operand[REG_OP2] = regnum2;
	registerFile_putWord(cpu->rf,cpu->operand[REG_OP2], address);
	cpu->operand[REG_OP1] = regnum;
	registerFile_putWord(cpu->rf,cpu->operand[REG_OP1], 0xffff);
	
	printf("Register %u has %x in the low byte before the load.\n", regnum, registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("The register should contain %x in the low byte after the load!\n", memory_getByte(cpu->mm, registerFile_getWord(cpu->rf,cpu->operand[REG_OP2], &error) + bp, &error));
	cpu_ldbBaseRelative(cpu);
	printf("Register %u has %x in it after the load.\n", regnum, registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));

	printf("\n\nTesting the LDB_INDEX execution\n");
	
	regnum = R1;
	regnum2 = R2;
	address = 0x8400;
	low = 0xab;
	ushort ip = registerFile_getWord(cpu->rf, IP0, &error);
	ip = address;
	memory_putByte(cpu->mm, ip, low);
	cpu->operand[REG_OP2] = regnum2;
	registerFile_putWord(cpu->rf,cpu->operand[REG_OP2], address);
	cpu->operand[REG_OP1] = regnum;
	registerFile_putWord(cpu->rf,cpu->operand[REG_OP1], 0x1234);
	
	printf("Register %u has %x in the low byte before the load.\n", regnum, registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("The register should contain %x in the low byte after the load!\n", memory_getByte(cpu->mm,ip, &error));
	cpu_ldbIndex(cpu);
	printf("Register %u has %x in it after the load.\n", regnum, registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	
	printf("\n\nTesting the LDB_INDIRECT execution\n");
	regnum = R1;
	regnum2 = R2;
	address = 0x8780;
	ushort address2 = 0x8750;
	low = 0xff;
	
	cpu->operand[REG_OP2] = regnum2;
	registerFile_putWord(cpu->rf,cpu->operand[REG_OP2], address2);
	memory_putWord(cpu->mm, registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error), address);
	memory_putByte(cpu->mm, memory_getWord(cpu->mm , registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error), &error), low);
	cpu->operand[REG_OP1] = regnum;
	registerFile_putWord(cpu->rf,cpu->operand[REG_OP1], 0x4687);
	
	printf("Register %u has %x in the low byte before the load.\n", regnum, registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("The register should contain %x in the low byte after the load!\n",memory_getByte(cpu->mm, memory_getWord(cpu->mm, registerFile_getWord(cpu->rf , cpu->operand[REG_OP2], &error),&error) , &error));
	cpu_ldbIndirect(cpu);
	printf("Register %u has %x in it after the load.\n", regnum, registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	
	printf("\n\nTesting the LDW_REGISTER execution\n");
	
	regnum = R1;
	regnum2 = R2;
	address = 0x8780;
	high = 0xff;
	low = 0xab;
	
	cpu->operand[REG_OP2] = regnum2;
	registerFile_putWord(cpu->rf,cpu->operand[REG_OP2], address);
	memory_putByte(cpu->mm, registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error), high);
	memory_putByte(cpu->mm, registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error) + 1 , low);
	
	printf("Register %u has %x in it before the load.\n", regnum, registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("The register should contain %x in it after the load!\n", 0xffab);
	cpu_ldwRegister(cpu);
	printf("Register %u has %x in it after the load.\n", regnum, registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));

	printf("\n\nTesting the LDW_BASE_RELATIVE execution\n");
	
	regnum = R1;
	regnum2 = R2;
	address = 0x8880;
	high = 0xab;
	low = 0xee;
    bp = registerFile_getWord(cpu->rf, BP, &error);
	
	cpu->operand[REG_OP2] = regnum2;
	registerFile_putWord(cpu->rf,cpu->operand[REG_OP2], address);
	memory_putByte(cpu->mm, registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error) + bp, high);
	memory_putByte(cpu->mm, (registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error) + bp) + 1, low);
	
	printf("Register %u has %x in it before the load.\n", regnum, registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("The register should contain %x in it after the load!\n", 0xabee);
	cpu_ldwBaseRelative(cpu);
	printf("Register %u has %x in it after the load.\n", regnum, registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	
	printf("\n\nTesting the LDW_INDEX execution\n");
	
	regnum = R1;
	regnum2 = R2;
	address = 0x8123;
	high = 0xef;
	low = 0xcd;
	ip = registerFile_getWord(cpu->rf, IP0, &error);
	
	memory_putByte(cpu->mm, ip, high);
	ip++;
	memory_putByte(cpu->mm, ip , low);
	
	printf("Register %u has %x in it before the load.\n", regnum, registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("The register should contain %x in it after the load!\n", 0xefcd);
	cpu_ldwIndex(cpu);
	printf("Register %u has %x in it after the load.\n", regnum, registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	
	printf("\n\nTesting the LDW_INDIRECT execution\n");
	// Rd(hob) <- M[M[Ra]]; Rd(lob) <- M[M[Ra + 1]] not working yet
	regnum = R1;
	regnum2 = R2;
	address = 0x8123;
	address2 = 0x8999;
	high = 0x12;
	low = 0x34;
	
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP2], 0x1229);
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP2] + 1, 0x1339);
	memory_putByte(cpu->mm, registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error), address);
	memory_putByte(cpu->mm, registerFile_getWord(cpu->rf, cpu->operand[REG_OP2] + 1, &error), address2);
	memory_putByte(cpu->mm, memory_getByte(cpu->mm, registerFile_getWord(cpu->rf, cpu->operand[REG_OP2], &error), &error), high);
	memory_putByte(cpu->mm, memory_getByte(cpu->mm, registerFile_getWord(cpu->rf, cpu->operand[REG_OP2] + 1, &error), &error), low);
	
	printf("Register %u has %u in it before the load.\n", regnum, registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("The register should contain %u in it after the load!\n", 0x1234);
	cpu_ldwIndex(cpu);
	printf("Register %u has %u in it after the load.\n", regnum, registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	
	
	printf("\n\nTesting mov execution\n");
	
	regnum = R1;
	regnum2 = R2;
	ushort value = 0xabcd;
	
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], 0xffff);
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP2], value); 
	
	printf("Register %u has %x in it before the load.\n", regnum, registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("The register should contain %x in it after the load!\n", value);
	cpu_mov(cpu);
	printf("Register %u has %x in it after the load.\n", regnum, registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));

    printf("\n\n");
	return;
}

/* Tests the store instructions.*/
void testST(CPUPtr cpu) {
	printf("\nTesting store byte register!\n");
	// M[Ra] <- (Rs(lob))
    
    ushort data = 0x1234;
    ushort addr = 0x5555;
	cpu->operand[REG_OP1] = RA;
	cpu->operand[REG_OP2] = RB;
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], data);
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP2], addr);
	
	printf("This is what is in the low byte of register %u before the store: 0x%02x\n", cpu->operand[REG_OP1], registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("This is the byte at address 0x%04x before the store: 0x%02x\n", addr, memory_getByte(cpu->mm, addr, &error));
	cpu_stbRegister(cpu);
	printf("This is the byte at address 0x%04x after the store: 0x%02x\n", addr, memory_getByte(cpu->mm, addr, &error)); 
    
    
	printf("\n\nTesting store byte base relative!\n");
	// M[BP + Ra] <- (Rs(lob))  Rs(lob), %Ra
	
    data = 0xabcd;
    addr = 0x3000;
    ushort base = 0x1000;
	cpu->operand[REG_OP1] = RC;
	cpu->operand[REG_OP2] = RD;
    registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], data);
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP2], addr);
    registerFile_putWord(cpu->rf, BP, base);

	printf("This is what is in the low byte of register %u before the store: 0x%02x\n", cpu->operand[REG_OP1], registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("This is the byte at address 0x%04x before the store: 0x%02x\n", base + addr, memory_getByte(cpu->mm, base + addr, &error));
	cpu_stbBaseRelative(cpu);
	printf("This is the byte at address 0x%04x after the store: 0x%02x\n", base + addr, memory_getByte(cpu->mm, base + addr, &error)); 
	
    
	printf("\n\nTesting store byte index!\n");
	// M[IP-1] <- (Rs(lob)); IP-1 <- IP-1 + 1  

    data = 0xabcd;
    addr = 0x30ab;
	cpu->operand[REG_OP1] = RC;
	cpu->operand[REG_OP2] = IP1;
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], data);
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP2], addr);
    
	printf("This is what is in the low byte of register %u before the store: 0x%02x\n", cpu->operand[REG_OP1], registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("This is the byte at address 0x%04x before the store: 0x%02x\n", addr, memory_getByte(cpu->mm, addr, &error));
    printf("This is the value of the IP1 register before the store: 0x%04x\n", registerFile_getWord(cpu->rf, IP1, &error));
	cpu_stbIndex(cpu);
	printf("This is the byte at address 0x%04x after the store: 0x%02x\n", addr, memory_getByte(cpu->mm, addr, &error)); 
    printf("This is the value of the IP1 register after the store: 0x%04x\n", registerFile_getWord(cpu->rf, IP1, &error));
	
	
	printf("\n\nTesting store byte indirect!\n");
	//Rs, @Ra		 M[M[Ra]] <- (Rs(lob))
	
    data = 0xf8cd;
	addr = 0x3554;
    ushort addr2 = 0x5999;
	cpu->operand[REG_OP1] = R1;
	cpu->operand[REG_OP2] = R0;
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], data);
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP2], addr);
	memory_putWord(cpu->mm, addr, addr2);
	
	printf("This is what is in the low byte of register %u before the store: 0x%02x\n", cpu->operand[REG_OP1], registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("This is the byte at address 0x%04x before the store: 0x%02x\n", addr2, memory_getByte(cpu->mm, addr2, &error));
	cpu_stbIndirect(cpu);
	printf("This is the byte at address 0x%04x after the store: 0x%02x\n", addr2, memory_getByte(cpu->mm, addr2, &error)); 
	
    
	printf("\n\nTesting store word register\n");
	//Rs, Ra		 M[Ra] <- (Rs(hob)); M[Ra + 1] <- (Rs(lob))

    data = 0x3457;
	addr = 0x5684;
	cpu->operand[REG_OP1] = R9;
	cpu->operand[REG_OP2] = R7;
    registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], data);
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP2], addr);
	
	printf("This is what is in register %u before the store: 0x%04x\n", cpu->operand[REG_OP1], registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("This is the word at address 0x%04x before the store: 0x%04x\n", addr, memory_getWord(cpu->mm, addr, &error));
	cpu_stwRegister(cpu);
	printf("This is the word at address 0x%04x after the store: 0x%04x\n", addr, memory_getWord(cpu->mm, addr, &error)); 
	
    
	printf("\n\nTesting store word base relative\n");
	//Rs, %Ra		// M[BP + Ra] <- (Rs(hob)); M[BP + Ra + 1] <- (Rs(lob))
    
    data = 0xabed;
	addr = 0x5684;
    base = 0x0333;
	cpu->operand[REG_OP1] = R3;
	cpu->operand[REG_OP2] = RD;
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], data);
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP2], addr);
	registerFile_putWord(cpu->rf, BP, base);
	
	printf("This is what is in register %u before the store: 0x%04x\n", cpu->operand[REG_OP1], registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("This is the word at address 0x%04x before the store: 0x%04x\n", addr, memory_getWord(cpu->mm, base + addr, &error));
	cpu_stwBaseRelative(cpu);
	printf("This is the word at address 0x%04x after the store: 0x%04x\n", addr, memory_getWord(cpu->mm, base + addr, &error));
	
    
    printf("\n\nTesting store word index!\n");
    data = 0xabcd;
    addr = 0x30ab;
	cpu->operand[REG_OP1] = R2;
	cpu->operand[REG_OP2] = IP1;
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], data);
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP2], addr);
    
	printf("This is what is in register %u before the store: 0x%04x\n", cpu->operand[REG_OP1], registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("This is the word at address 0x%04x before the store: 0x%04x\n", addr, memory_getWord(cpu->mm, addr, &error));
    printf("This is the value of the IP1 register before the store: 0x%04x\n", registerFile_getWord(cpu->rf, IP1, &error));
	cpu_stwIndex(cpu);
	printf("This is the word at address 0x%04x after the store: 0x%04x\n", addr, memory_getWord(cpu->mm, addr, &error)); 
    printf("This is the value of the IP1 register after the store: 0x%04x\n", registerFile_getWord(cpu->rf, IP1, &error));
	
	
	printf("\n\nTesting store word indirect!\n");
    data = 0xf8cd;
	addr = 0x3554;
    addr2 = 0x5999;
	cpu->operand[REG_OP1] = RC;
	cpu->operand[REG_OP2] = RD;
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], data);
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP2], addr);
	memory_putWord(cpu->mm, addr, addr2);
	
	printf("This is what is in register %u before the store: 0x%04x\n", cpu->operand[REG_OP1], registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("This is the word at address 0x%04x before the store: 0x%04x\n", addr2, memory_getWord(cpu->mm, addr2, &error));
	cpu_stwIndirect(cpu);
	printf("This is the word at address 0x%04x after the store: 0x%04x\n", addr2, memory_getWord(cpu->mm, addr2, &error)); 
    
    printf("\n\n");
	return;
}

/* Tests the push and pop instructions.*/
void testPushPop(CPUPtr cpu) {
	
	printf("\nTesting the pushByte operation\n");
	
	ushort regnum = RA;
	ushort value = 0x55;
	cpu->operand[REG_OP1] = regnum;
	registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], value);
	registerFile_putWord(cpu->rf, SP, 0x5000);
	
	printf("This is what is in the low byte of the register before the push: %02x\n", registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	cpu_pushb(cpu);
	ushort sp = registerFile_getWord(cpu->rf, SP, &error);
	printf("This is what is at the top of the stack after the push: %02x\n", memory_getByte(cpu->mm, sp, &error));
	
	
	printf("\n\nTesting the popByte operation!\n");
	value = 0xab;
	registerFile_putLowByte(cpu->rf, cpu->operand[REG_OP1], value);
    sp = registerFile_getWord(cpu->rf, SP, &error);
	printf("This is what is in the low byte of the register before the pop: %02x\n", registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("This is what is at the top of the stack before the pop: %02x\n", memory_getByte(cpu->mm, sp, &error));
	cpu_popb(cpu);
	printf("This is what is in the register after the pop: %02x\n", registerFile_getLowByte(cpu->rf, cpu->operand[REG_OP1], &error));
	
	
	printf("\n\nTesting the push word operation!\n");
	value = 0xabcd;
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], value);
    sp = registerFile_getWord(cpu->rf, SP, &error);
    
	printf("This is what is in the register before the push: %04x\n", registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	cpu_pushw(cpu);
	sp = registerFile_getWord(cpu->rf, SP, &error);
	printf("This is what is at the top of the stack after the push: %04x\n", memory_getWord(cpu->mm, sp, &error));
	
	
	printf("\n\nTesting the pop word operation!\n");
	value = 0xff34;
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], value);
    sp = registerFile_getWord(cpu->rf, SP, &error);
    
	printf("This is what is in the register before the pop: %x\n", registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	printf("This is what is at the top of the stack before the pop: %x\n", memory_getWord(cpu->mm, sp, &error));
	cpu_popw(cpu);
	printf("This is what is in the register after the pop: %x\n", registerFile_getWord(cpu->rf, cpu->operand[REG_OP1], &error));
	
    printf("\n\n");
	return;
}

/* Tests the branch instructions.*/
void testBranch(CPUPtr cpu) {
	
	printf("\nTesting branch Immediate!\n");
	ushort address = 0x333;
	cpu->operand[IMMED11] = address;
	register_putValue(&cpu->PC, 0x3000);
	
	printf("This is what im trying to change PC to: %x \n", 0x3000 + 0x333);
	printf("This is what PC is before: %x\n", register_getValue(&cpu->PC,&error));
	cpu_bri(cpu);
	printf("This is what PC is now: %x\n", register_getValue(&cpu->PC,&error));
	
	printf("\n\nTesting unconditional branch!\n");
	register_putValue(&cpu->PC, 0x3000);
	address = 0x8585;
	cpu->operand[REG_OP1] = RA;
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1],address);
	printf("This is what im trying to change PC to: %x \n", registerFile_getWord(cpu->rf,cpu->operand[REG_OP1],&error));
	printf("This is what PC is before: %x\n",  register_getValue(&cpu->PC,&error));
	cpu_br(cpu);
	printf("This is what PC is now: %x\n",  register_getValue(&cpu->PC,&error));
	
	
	printf("\n\nTesting overflow immediate branch!\n");
	register_putValue(&cpu->PC, 0x3000);
	address = 0xcd;
	cpu->operand[IMMED9] = address;
	cpu->operand[BR_MODE] = 1;
	register_putLowByte(&cpu->SW, 1);
	printf("This is what im trying to change PC to: %x \n", 0x3000 + 0xcd);
	printf("This is what PC is before: %x\n", register_getValue(&cpu->PC,&error));
	cpu_brix(cpu);
	printf("This is what PC is now: %x\n", register_getValue(&cpu->PC,&error));
	
	printf("\n\nTesting overflow Register branch!\n");
	register_putValue(&cpu->PC, 0x3000);
	address = 0xaabc;
	cpu->operand[BR_MODE] = 1;
	register_putLowByte(&cpu->SW, 1);
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], address);
	printf("This is what im trying to change PC to: %x \n", 0xaabc);
	printf("This is what PC is before: %x\n", register_getValue(&cpu->PC,&error));
	cpu_brx(cpu);
	printf("This is what PC is now: %x\n", register_getValue(&cpu->PC,&error));
	
    printf("\n\n");
	return;
}

/* Tests the jump and return instructions.*/
void testJumps(CPUPtr cpu) {
	
	printf("\nTesting JSR! \n");
	
	ushort address = 0xabcd;
	ushort SPaddress = 0x8500;
	ushort regnum = RA;
	cpu->operand[REG_OP1] = regnum;
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], address);
	registerFile_putWord(cpu->rf, SP, SPaddress);
	ushort sp = registerFile_getWord(cpu->rf, SP, &error);
	
	printf("This is what we expect to be in the pc after: %x\n", address);
	printf("This is what is in the PC before: %x \n", register_getValue(&cpu->PC, &error));
	printf("This is what is in the memory spot before the push: %x\n", memory_getWord(cpu->mm, sp, &error));
	cpu_jsr(cpu);
    sp = registerFile_getWord(cpu->rf, SP, &error);
	printf("This is what is in the PC after: %x \n", register_getValue(&cpu->PC, &error));
	printf("This is what is in the memory spot after the push: %x\n", memory_getWord(cpu->mm, sp, &error));
	
	printf("\n\nTesting JSRX! \n");
	
	address = 0x1234;
	regnum = RB;
	cpu->operand[REG_OP1] = regnum;
	registerFile_putWord(cpu->rf, cpu->operand[REG_OP1], address);
	cpu->operand[BR_MODE] = 1;
	register_putLowByte(&cpu->SW, 1);
    sp = registerFile_getWord(cpu->rf, SP, &error);
	
	printf("This is what we expect to be in the pc after: %x\n", address);
	printf("This is what is in the PC before: %x \n", register_getValue(&cpu->PC, &error));
	printf("This is what is in the memory spot before the push: %x\n", memory_getWord(cpu->mm, sp, &error));
	cpu_jsrx(cpu);
    sp = registerFile_getWord(cpu->rf, SP, &error);
	printf("This is what is in the PC after: %x \n", register_getValue(&cpu->PC, &error));
	printf("This is what is in the memory spot after the push: %x\n", memory_getWord(cpu->mm, sp, &error));
	
	printf("\n\nTesting RETURN! \n");
	
	regnum = RB;
	cpu->operand[REG_OP1] = regnum;
	memory_putWord(cpu->mm, SP, address);
    sp = registerFile_getWord(cpu->rf, SP, &error);
	
	printf("This is what we expect to be in the pc: %x\n", memory_getWord(cpu->mm, sp, &error));
	printf("This is what is in the PC: %x \n", register_getValue(&cpu->PC, &error));
	printf("This is what is in the memory spot before the pop: %x\n", memory_getWord(cpu->mm, sp, &error));
	cpu_ret(cpu);
	printf("This is what is in the PC after: %x \n", register_getValue(&cpu->PC, &error));
	
    printf("\n\n");
	return;
}

/* Prints a banner with a title. */
void printBanner(char* title) {
    int i;
    for (i = 0; i < 80; ++i)
    {
        putchar('*');
    }

    printf("\n* %s", title);
 
    int spaces = 80 - strlen(title) - 3;
    for (i = 0; i < spaces; ++i)
    {
        putchar(' ');
    }

    printf("*\n");

    for (i = 0; i < 80; ++i)
    {
        putchar('*');
    }
}

